#include "akm.h"
#include "math.h"

#define L 0.143f            // 小车前后轮之间的距离（米）轴距
#define W 0.114f            // 左右轮之间的距离（米）
#define K 0.051             // 长杆长度（m）
#define SERVO_WHEEL 1.5625f // 舵机角度和轮子角度的比值
#define Pi 3.1415926        // π

float Target_A;
float Target_B;
float Servo;

/**************************************************************************
函数功能：小车运动数学模型
入口参数：速度和角度??
返回  值：无
**************************************************************************/

// void Akm_analysis(float velocity, float turn) {
//   Servo = turn; // 舵机转向
//   int tan_turn = turn - 90;
//   Target_A = velocity * (1 + T * tan(tan_turn) / 2 / L);
//   Target_B = velocity * (1 - T * tan(tan_turn) / 2 / L); // 后轮差速
// }

/**************************************************************************
函数功能：阿克曼运动学计算
入口参数：小车速度(m/s)与转向半径(m),输出舵机角度和AB轮速度
返回  值：无
**************************************************************************/
void Akm_cal_R(float velocity, float R) {
  float angle_v = velocity / R; // 角速度
  float turn =
      atan(L / (R - (K / 2))); // 前轮角度（弧度制）,tant传入的参数为角度制
  turn = turn * (180 / Pi);          // 前轮角度（转为角度制）
  Servo = (turn * SERVO_WHEEL) + 90; // 输入给舵机的角度
  Target_A = velocity - angle_v * (W / 2);
  Target_B = velocity + angle_v * (W / 2);
}

/**************************************************************************
函数功能：阿克曼运动学计算
入口参数：小车速度(m/s)与前轮转角（°）,输出舵机角度和AB轮速度
返回  值：无
**************************************************************************/
void Akm_cal_A(float velocity, float a) {
  // if(a>30)a=30;
  // if(a<-30)a=30;
  float R = L / atan(a);
  float angle_v = velocity / R;         // 角速度
  float turn = atan(L / (R - (K / 2))); // 前轮角度（弧度制）
  turn = turn * (180 / Pi);             // 前轮角度（转为角度制）
  Servo = (a * SERVO_WHEEL) + 90;    // 输入给舵机的角度
  Target_A = velocity - angle_v * (W / 2);
  Target_B = velocity + angle_v * (W / 2);
}

/**************************************************************************
函数功能：将脉冲数转换为m
入口参数：小车速度(m/s) 
返回  值：应该设置的脉冲数在50ms中断
**************************************************************************/
float Pulse_V(float V)
{
  float Pulse_v = V * 182.4191;
  return Pulse_v;
}
